#include "fault_code.h"

FaultDeviceWingbot_T WingbotFault;

u8 FaultLedLink()
{
    u8 cnt = 0;
    //目前只有超声和电机，所以实际Ledlen只会轮询到2，只有2个是有意义的
    for (u8 i = 0; i < Ledlen; i++)
    {
        //只要有一个报严重故障，则红灯常亮
        if (WingbotFault.FaultLed.FaultLedDeep[i] == Bad)
        {
            cnt++;
            LED3 = 1;
            return Bad;
        }
    }
    for (u8 i = 0; i < Ledlen; i++)
    {
        //否则只要有一个处于中等故障，则红灯闪烁
        if (WingbotFault.FaultLed.FaultLedDeep[i] == Medium)
        {
            cnt++;
            LED3 = 0;
            return Medium;
        }
    }
    for (u8 i = 0; i < Ledlen; i++)
    {
        //否则只要有一个处于轻微故障，则红灯闪烁
        if (WingbotFault.FaultLed.FaultLedDeep[i] == Slight)
        {
            cnt++;
            LED3 = 0;
            return Slight;
        }
    }

    if (cnt == 0) //无错误情况下系统状态灯打开，故障灯熄灭
    {
        LED3 = 0; //否则故障灯熄灭
        return 0;
    }
    else
    {
        return 1;
    }
}

//u8 StatusLedLink()
//{
//      if(WingbotFault.PanasonicMotorWingbot.CarVel==0)
//      {
//        SYS_STATE_Led_OFF();           //系统状态灯关闭
//      }
//      else
//      {
//         SYS_STATE_Led_OPEN();       //系统状态灯打开
//      }
//      return 1;
//}

/**
  * @brief  检查单个编码器有无报警   注意：因为传经来的ID为从2开始，数组只有4个元素，所以ID号要减2 ，防止数组越界
  * @param  encoder_t：编码器结构体
  * @retval 编码器错误状态：不为0有报警，0无报警
  */
u8 CheckEncoderFault(Encoder *encoder_t)
{
    u8 ID = 0;
    ID = encoder_t->ID - 2; //注意：因为编码器ID为从2开始，放进数组第一个元素，所以这里ID要减2 ，以免数组越界 ！！！！

    //在原来基础上增加故障码指示
    if (encoder_t->Encoder_CAN_Read_OK) //判断是否为编码器读取超时
    {
        //刷新当前故障状态
        WingbotFault.EncoderWingbot.EncoderSensor[ID].now_err = TimeOut;
        //按位或0x01,刷新总的故障状态
        WingbotFault.EncoderWingbot.EncoderSensor[ID].err_status = WingbotFault.EncoderWingbot.EncoderSensor[ID].err_status | 0x01;
        //错误计数加1
        WingbotFault.EncoderWingbot.EncoderSensor[ID].err_cnt++;
        //若累加越界则清零错误计数
        if (WingbotFault.EncoderWingbot.EncoderSensor[ID].err_cnt > 30000000)
        {
            WingbotFault.FaultLed.FaultLedDeep[PriEncoder] = Bad; //更新故障程度为严重
        }
        WingbotFault.FaultLed.FaultLedDeep[PriEncoder] = Bad; //更新故障程度为中等
    }
    else
    {
        //按位与0xFE  1111 1110   ,刷新总的故障状态
        WingbotFault.EncoderWingbot.EncoderSensor[ID].err_status = WingbotFault.EncoderWingbot.EncoderSensor[ID].err_status & 0xFE; //将最后一置0，清除报警
                                                                                                                                    //刷新当前故障状态
        WingbotFault.EncoderWingbot.EncoderSensor[ID].now_err = NoErr;
        if (WingbotFault.EncoderWingbot.EncoderSensor[ID].err_cnt > 0)
        {
            WingbotFault.EncoderWingbot.EncoderSensor[ID].err_cnt--; //防止一直故障，cnt累加数据溢出 ,每正常一次错误数减一
        }
    }

    if (encoder_t->angle > 5000) //判断是否为编码器超过阈值
    {
        //刷新当前故障状态
        WingbotFault.EncoderWingbot.EncoderSensor[ID].now_err = Higher;
        //按位或0x01,刷新总的故障状态
        WingbotFault.EncoderWingbot.EncoderSensor[ID].err_status = WingbotFault.EncoderWingbot.EncoderSensor[ID].err_status | 0x02;
        //错误计数加1
        WingbotFault.EncoderWingbot.EncoderSensor[ID].err_cnt++;
        //若累加越界则清零错误计数
        if (WingbotFault.EncoderWingbot.EncoderSensor[ID].err_cnt > 30000000)
        {
            WingbotFault.FaultLed.FaultLedDeep[PriEncoder] = Bad; //更新故障程度为严重
        }
        WingbotFault.FaultLed.FaultLedDeep[PriEncoder] = Medium; //更新故障程度为中等
    }
    else
    {
        //按位与0xFE  1111 1101   ,刷新总的故障状态
        WingbotFault.EncoderWingbot.EncoderSensor[ID].err_status = WingbotFault.EncoderWingbot.EncoderSensor[ID].err_status & 0xFD; //将最后一置0，清除报警
                                                                                                                                    //刷新当前故障状态
        WingbotFault.EncoderWingbot.EncoderSensor[ID].now_err = NoErr;
        if (WingbotFault.EncoderWingbot.EncoderSensor[ID].err_cnt > 0)
        {
            WingbotFault.EncoderWingbot.EncoderSensor[ID].err_cnt--; //防止一直故障，cnt累加数据溢出 ,每正常一次错误数减一
        }
    }

    if (encoder_t->angle < -5000) //判断是否为编码器超过阈值
    {
        //刷新当前故障状态
        WingbotFault.EncoderWingbot.EncoderSensor[ID].now_err = Lower;
        //按位或0x01,刷新总的故障状态
        WingbotFault.EncoderWingbot.EncoderSensor[ID].err_status = WingbotFault.EncoderWingbot.EncoderSensor[ID].err_status | 0x04;
        //错误计数加1
        WingbotFault.EncoderWingbot.EncoderSensor[ID].err_cnt++;
        //若累加越界则清零错误计数
        if (WingbotFault.EncoderWingbot.EncoderSensor[ID].err_cnt > 30000000)
        {
            WingbotFault.FaultLed.FaultLedDeep[PriEncoder] = Bad; //更新故障程度为严重
        }
        WingbotFault.FaultLed.FaultLedDeep[PriEncoder] = Medium; //更新故障程度为中等
    }
    else
    {
        //按位与0xFE  1111 1011   ,刷新总的故障状态
        WingbotFault.EncoderWingbot.EncoderSensor[ID].err_status = WingbotFault.EncoderWingbot.EncoderSensor[ID].err_status & 0xFB; //将最后一置0，清除报警
                                                                                                                                    //刷新当前故障状态
        WingbotFault.EncoderWingbot.EncoderSensor[ID].now_err = NoErr;
        if (WingbotFault.EncoderWingbot.EncoderSensor[ID].err_cnt > 0)
        {
            WingbotFault.EncoderWingbot.EncoderSensor[ID].err_cnt--; //防止一直故障，cnt累加数据溢出 ,每正常一次错误数减一
        }
    }

    //如果发生报警，则报警传感器个数+1
    if (WingbotFault.EncoderWingbot.EncoderSensor[ID].err_status != M_NoErr)
    {
        WingbotFault.EncoderWingbot.FaultSensor.FaultSensorCnt++;
    }

    return WingbotFault.EncoderWingbot.EncoderSensor[ID].err_status;
}

u8 UpdateEncoderFaultDepp() //更新ENCODER故障
{
    for (u8 i = 0; i < 4; i++)
    {
        if ((WingbotFault.EncoderWingbot.EncoderSensor[i].err_status & 0x01) != 0)
        {
            WingbotFault.FaultLed.FaultLedDeep[PriEncoder] = Bad;
        }
    }
    if (WingbotFault.EncoderWingbot.FaultSensor.FaultSensorCnt != 0)
    {
        if (WingbotFault.FaultLed.FaultLedDeep[PriEncoder] != Bad) //如果故障程度不是严重，且故障传感器个数不为0
        {
            WingbotFault.FaultLed.FaultLedDeep[PriEncoder] = Medium; //则更新故障程度为中等
        }
        WingbotFault.FaultLed.Led[PriEncoder] = WingbotFault.EncoderWingbot.FaultSensor.FaultSensorCnt; //将故障传感器个数传入LED指示数组
        WingbotFault.EncoderWingbot.FaultSensor.FaultSensorCnt = 0;                                     //本次循环故障清零
        return 1;
    }
    else
    {
        WingbotFault.FaultLed.Led[PriEncoder] = WingbotFault.EncoderWingbot.FaultSensor.FaultSensorCnt; //将故障传感器个数传入LED指示数组
        WingbotFault.EncoderWingbot.FaultSensor.FaultSensorCnt = 0;                                     //本次循环故障清零
        WingbotFault.FaultLed.FaultLedDeep[PriEncoder] = Good;                                          //更新故障程度
        return 0;
    }
}

/**
  * @brief  更新编码器故障程度
  */
u8 UpdateFaultDepp()
{
    UpdateEncoderFaultDepp(); //更新ENCODER故障
    UpdateImuFaultDepp();     //更新IMU故障深度
    UpdateUartFaultDepp();    //更新串口故障深度
    return 1;
}

/**
  * @brief  20ms循环函数
  */
void loop20ms()
{
    if (SysTime.Time1_Flag == 1)
    {
        loop_qurey_angle();
        Analysis_Order();
        SysTime.Time1_Flag = 0; //刷新标志
    }
}

/**
  * @brief  200ms循环函数
  */
void loop200ms()
{
    if (SysTime.Time10_falg == 1)
    {
        CheckImuFault(openImu); //监测imu故障
        CheckUartFault();       //监测串口故障
        UpdateFaultDepp();
        wingbowHost->AnalyWingbowHost();
        SysTime.Time10_falg = 0; //刷新标志
    }
}

/**
  * @brief  主循环函数
  */
void mainloop()
{
    //   OpenImu::get()->BurstModeAcc_32Bit();
    OpenImu::get()->UartanalysisImuData();
    loop_qurey_angle();
    Analysis_Order();
}

u8 CheckImuTimeOut(OpenImu *Adis)
{
    WingbotFault.ImuWingbot.FaultSensor.TimeOutFlag = SpiTimeOut; //更新IMU超时标志

    if (WingbotFault.ImuWingbot.FaultSensor.TimeOutFlag > 0) //判断IMU SPI通信是否超时
    {
        //刷新当前故障状态
        WingbotFault.ImuWingbot.Imu[0].now_err = TimeOut;
        //按位或0x01,刷新总的故障状态
        WingbotFault.ImuWingbot.Imu[0].err_status = WingbotFault.ImuWingbot.Imu[0].err_status | 0x01;
        //错误计数加1
        WingbotFault.ImuWingbot.Imu[0].err_cnt++;
        //若累加越界则清零错误计数
        if (WingbotFault.ImuWingbot.Imu[0].err_cnt > 40)
        {
            WingbotFault.FaultLed.FaultLedDeep[PriImu] = Bad; //更新故障程度为严重
        }
        WingbotFault.FaultLed.FaultLedDeep[PriImu] = Medium; //更新故障程度为中等
    }
    else
    {
        //按位与0xFE  1111 1110   ,刷新总的故障状态
        WingbotFault.ImuWingbot.Imu[0].err_status = WingbotFault.ImuWingbot.Imu[0].err_status & 0xFE; //将最后一置0，清除报警
                                                                                                      //刷新当前故障状态
        WingbotFault.ImuWingbot.Imu[0].now_err = NoErr;
        if (WingbotFault.ImuWingbot.Imu[0].err_cnt > 0)
        {
            WingbotFault.ImuWingbot.Imu[0].err_cnt--; //防止一直故障，cnt累加数据溢出 ,每正常一次错误数减一
        }
    }

    return WingbotFault.ImuWingbot.Imu[0].err_status;
}

u8 CheckImuFault(OpenImu *Adis)
{
    if (Adis->device_id != Adis->OpenImu_Data.Imu_Id) //判断IMUID是否正确
    {
        //刷新当前故障状态
        WingbotFault.ImuWingbot.Imu[0].now_err = IdErr;
        //按位或0x01,刷新总的故障状态
        WingbotFault.ImuWingbot.Imu[0].err_status = WingbotFault.ImuWingbot.Imu[0].err_status | 0x04;
        //错误计数加1
        WingbotFault.ImuWingbot.Imu[0].err_cnt++;
        //若累加越界则清零错误计数
        if (WingbotFault.ImuWingbot.Imu[0].err_cnt > 30000000)
        {
            WingbotFault.FaultLed.FaultLedDeep[PriImu] = Bad; //更新故障程度为严重
        }
        WingbotFault.FaultLed.FaultLedDeep[PriImu] = Bad; //更新故障程度为中等
    }
    else
    {
        //按位与0xFE  1111 1011   ,刷新总的故障状态
        WingbotFault.ImuWingbot.Imu[0].err_status = WingbotFault.ImuWingbot.Imu[0].err_status & 0xFB; //将最后一置0，清除报警
                                                                                                      //刷新当前故障状态
        WingbotFault.ImuWingbot.Imu[0].now_err = NoErr;
        if (WingbotFault.ImuWingbot.Imu[0].err_cnt > 0)
        {
            WingbotFault.ImuWingbot.Imu[0].err_cnt--; //防止一直故障，cnt累加数据溢出 ,每正常一次错误数减一
        }
    }

    if (Adis->OpenImu_Data.temperature >= 26) //判断温度是否过高
    {
        //刷新当前故障状态
        WingbotFault.ImuWingbot.Imu[0].now_err = Higher;
        //按位或0x01,刷新总的故障状态
        WingbotFault.ImuWingbot.Imu[0].err_status = WingbotFault.ImuWingbot.Imu[0].err_status | 0x02;
        //错误计数加1
        WingbotFault.ImuWingbot.Imu[0].err_cnt++;
        //若累加越界则清零错误计数
        if (WingbotFault.ImuWingbot.Imu[0].err_cnt > 30000000)
        {
            WingbotFault.FaultLed.FaultLedDeep[PriImu] = Bad; //更新故障程度为严重
        }
        WingbotFault.FaultLed.FaultLedDeep[PriImu] = Medium; //更新故障程度为中等
    }
    else
    {
        //按位与0xFE  1111 1101   ,刷新总的故障状态
        WingbotFault.ImuWingbot.Imu[0].err_status = WingbotFault.ImuWingbot.Imu[0].err_status & 0xFD; //将最后一置0，清除报警
                                                                                                      //刷新当前故障状态
        WingbotFault.ImuWingbot.Imu[0].now_err = NoErr;
        if (WingbotFault.ImuWingbot.Imu[0].err_cnt > 0)
        {
            WingbotFault.ImuWingbot.Imu[0].err_cnt--; //防止一直故障，cnt累加数据溢出 ,每正常一次错误数减一
        }
    }

    CheckImuTimeOut(Adis); //监测通信是否超时

    //如果发生报警，则报警传感器个数+1
    if (WingbotFault.ImuWingbot.Imu[0].err_status != M_NoErr)
    {
        WingbotFault.ImuWingbot.FaultSensor.FaultSensorCnt++;
    }

    return WingbotFault.ImuWingbot.Imu[0].err_status;
}

/**
  * @brief  更新Imu故障程度
  */
u8 UpdateImuFaultDepp()
{
    for (u8 i = 0; i < 4; i++)
    {
        if ((WingbotFault.ImuWingbot.Imu[0].err_status & 0x01) != 0)
        {
            WingbotFault.FaultLed.FaultLedDeep[PriImu] = Bad;
        }
    }
    if (WingbotFault.ImuWingbot.FaultSensor.FaultSensorCnt != 0)
    {
        if (WingbotFault.FaultLed.FaultLedDeep[PriImu] != Bad) //如果故障程度不是严重，且故障传感器个数不为0
        {
            WingbotFault.FaultLed.FaultLedDeep[PriImu] = Medium; //则更新故障程度为中等
        }
        WingbotFault.FaultLed.Led[PriImu] = WingbotFault.ImuWingbot.FaultSensor.FaultSensorCnt; //将故障传感器个数传入LED指示数组
        WingbotFault.ImuWingbot.FaultSensor.FaultSensorCnt = 0;                                 //本次循环故障清零
        return 1;
    }
    else
    {
        WingbotFault.FaultLed.Led[PriImu] = WingbotFault.ImuWingbot.FaultSensor.FaultSensorCnt; //将故障传感器个数传入LED指示数组
        WingbotFault.ImuWingbot.FaultSensor.FaultSensorCnt = 0;                                 //本次循环故障清零
        WingbotFault.FaultLed.FaultLedDeep[PriImu] = Good;                                      //更新故障程度
        return 0;
    }
}

u8 CheckUartFault()
{

    if (WingbotFault.UartWingbot.FaultSensor.TimeOutFlag) //判断IMU SPI通信是否超时
    {
        //刷新当前故障状态
        WingbotFault.UartWingbot.Uart[0].now_err = TimeOut;
        //按位或0x01,刷新总的故障状态
        WingbotFault.UartWingbot.Uart[0].err_status = WingbotFault.UartWingbot.Uart[0].err_status | 0x01;
        //错误计数加1
        WingbotFault.UartWingbot.Uart[0].err_cnt++;
        //若累加越界则清零错误计数
        if (WingbotFault.UartWingbot.Uart[0].err_cnt > 40)
        {
            WingbotFault.FaultLed.FaultLedDeep[PriUart] = Bad; //更新故障程度为严重
        }
        WingbotFault.FaultLed.FaultLedDeep[PriUart] = Medium; //更新故障程度为中等
    }
    else
    {
        //按位与0xFE  1111 1110   ,刷新总的故障状态
        WingbotFault.UartWingbot.Uart[0].err_status = WingbotFault.UartWingbot.Uart[0].err_status & 0xFE; //将最后一置0，清除报警
                                                                                                          //刷新当前故障状态
        WingbotFault.UartWingbot.Uart[0].now_err = NoErr;
        if (WingbotFault.UartWingbot.Uart[0].err_cnt > 0)
        {
            WingbotFault.UartWingbot.Uart[0].err_cnt--; //防止一直故障，cnt累加数据溢出 ,每正常一次错误数减一
        }
    }

    //如果发生报警，则报警传感器个数+1
    if (WingbotFault.UartWingbot.Uart[0].err_status != M_NoErr)
    {
        WingbotFault.UartWingbot.FaultSensor.FaultSensorCnt++;
    }

    return WingbotFault.UartWingbot.Uart[0].err_status;
}

/**
  * @brief  更新Uart故障程度
  */
u8 UpdateUartFaultDepp()
{
    for (u8 i = 0; i < 4; i++)
    {
        if ((WingbotFault.UartWingbot.Uart[0].err_status & 0x01) != 0)
        {
            WingbotFault.FaultLed.FaultLedDeep[PriUart] = Bad;
        }
    }
    if (WingbotFault.UartWingbot.FaultSensor.FaultSensorCnt != 0)
    {
        if (WingbotFault.FaultLed.FaultLedDeep[PriUart] != Bad) //如果故障程度不是严重，且故障传感器个数不为0
        {
            WingbotFault.FaultLed.FaultLedDeep[PriUart] = Medium; //则更新故障程度为中等
        }
        WingbotFault.FaultLed.Led[PriUart] = WingbotFault.UartWingbot.FaultSensor.FaultSensorCnt; //将故障传感器个数传入LED指示数组
        WingbotFault.UartWingbot.FaultSensor.FaultSensorCnt = 0;                                  //本次循环故障清零
        return 1;
    }
    else
    {
        WingbotFault.FaultLed.Led[PriUart] = WingbotFault.UartWingbot.FaultSensor.FaultSensorCnt; //将故障传感器个数传入LED指示数组
        WingbotFault.UartWingbot.FaultSensor.FaultSensorCnt = 0;                                  //本次循环故障清零
        WingbotFault.FaultLed.FaultLedDeep[PriUart] = Good;                                       //更新故障程度
        return 0;
    }
}
